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Abstract:  One  of  the  ultimate  goals  of  robotics  research  is  to  create  autonomous  robots. 
Progress  toward  this  goal  requires  advances  in  many  domains,  including  automatic  motion 
planning.  The  “basic  problem ”  in  motion  planning  is  to  construct  a  collision-free  path  for 
a  moving  object  among  fixed  obstacles.  Several  approaches  have  been  proposed,  including 
cell  decomposition,  retraction,  and  potential  field.  Nevertheless,  most  existing  planners  still 
lack  efficiency,  or  reliability,  or  both.  In  this  paper,  we  consider  one  of  the  most  popular 
approaches  to  path  planning:  hierarchical  approximate  cell  decomposition.  We  propose  a  set 
of  new  algorithms  for  constructing  more  efficient  and  reliable  path  planners  based  on  this 
general  approach.  These  algorithms  concern  the  hierarchical  decomposition  of  the  robot’s 
configuration  space  into  rectangloid  cells,  and  the  search  of  the  connectivity  graphs  built 
at  each  level  of  decomposition.  We  have  implemented  these  algorithms  in  a  path  planner 
and  experimented  with  this  planner  on  various  examples.  Some  are  described  in  the  paper. 
These  experiments  show  that  our  planner  is  significantly  faster  than  previous  planners  based 
on  the  same  general  approach. 
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1  Introduction 


One  of  the  ultimate  goals  of  robotics  research  is  to  create  autonomous  robots.  Such 
robots  will  accept  high-level  descriptions  of  tasks  and  will  execute  them  without 
further  human  intervention.  The  input  descriptions  will  specify  what  the  user  wants 
done,  rather  than  how  to  do  it.  Progress  toward  this  goal  requires  advances  in  many 
domains,  including  automatic  reasoning,  perception,  and  real-time  control.  One  of 
the  key  topics  in  reasoning  is  motion  planning.  It  is  aimed  at  providing  robots  with 
the  capability  of  deciding  which  motion  commands  to  execute  in  order  to  achieve 
specified  arrangements  of  physical  objects.  During  the  last  ten  years,  it  has  emerged 
as  a  major  research  area  [Lozano- Perez,  1983]  [Schwartz,  Hopcroft  and  Sharir,  1987]. 


The  “basic  problem” 
fixed  obstacles  -  has 
proposed,  e.g.:  exact 


-  planning  a  collision-free  path  for  a  moving  object  among 
attracted  a  lot  of  attention.  Several  approaches  have  been 
cell  decomposition  [Schwartz  and  Sharir,  1983],  approximate 
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cell  decomposition  [Brooks  and  Lozano-Perez,  1983],  retraction  [O’Dunlaing,  Sharir 
and  Yap,  1983],  potential  field  [Khatib,  1986]  [Khosla  and  Volpe,  1988]  [Rimon  and 
Koditschek,  1988].  Most  of  the  existing  methods,  however,  still  lack  efficiency,  or 
reliability,  or  both,  in  practical  cases. 

In  this  paper,  we  consider  one  of  the  most  popular  approaches  to  motion  planning: 
hierarchical  approximate  cell  decomposition.  It  was  first  introduced  in  [Brooks  and 
Lozano-Perez,  1983],  with  subsequent  contributions  by  other  authors  (e.g.,  [Laugier 
and  Germain,  1985]  (Faverjon,  1986]  [Kambhampati  and  Davis,  1986]).  We  propose 
a  set  of  new  algorithms  for  constructing  more  efficient  path  planners  based  on  this 
approach.  We  have  implemented  these  algorithms  in  a  planner  and  experimented 
with  them  on  a  variety  of  examples.  Taking  into  account  the  relative  speed  of  the 
computers,  our  planner  is  significantly  (approximately  10  times)  faster  than  previous 
planners  using  the  same  approach.  Like  other  path  planners  based  on  cell  decomposi¬ 
tion,  our  planner  generates  a  sequence  of  cells,  which  we  call  a  “channel”.  Hence,  the 
robot  does  not  commit  itself  to  a  single  path  at  planning  time,  so  that  it  can  adapt  its 
path  at  execution  time,  using,  say,  a  potential  field  method  for  avoiding  unexpected 
obstacles.  We  have  implemented  this  approach  on  an  actual  mobile  robot  moving  in 
a  time  varying  office  environment  [Choi,  Zhu  and  Latombe,  1989]. 

The  hierarchical  approximate  cell  decomposition  approach  consists  of  decomposing 
the  configuration  space  of  the  moving  object  into  rectangloid  cells  at  successive  lev¬ 
els  of  approximation.  Cells  are  classified  to  be  EMPTY  or  FULL,  depending  on 
whether  they  lie  entirely  outside  or  entirely  inside  the  obstacles.  If  they  are  neither 
EMPTY,  nor  FULL,  they  are  labelled  MIXED.  At  each  level  of  approximation,  the 
planner  searches  the  graph  of  the  adjacency  relation  among  the  cells  for  a  sequence 
of  adjacent  EMPTY  cells  connecting  the  initial  configuration  of  the  robot  to  the  goal 
configuration.  If  no  such  sequence  is  found,  it  decomposes  some  MIXED  cells  into 
smaller  cells,  label  them  appropriately,  and  searches  again  for  a  sequence  of  EMPTY 
cells.  The  process  ends  when  a  solution  has  been  found,  or  it  is  guaranteed  that  no 
solution  can  be  found,  or  MIXED  cells  are  smaller  than  some  prespecified  size. 

Most  motion  planning  approaches  have  their  own  advantages  and  drawbacks,  which 
have  to  be  weighted  in  function  of  the  context  in  which  they  are  considered.  Never¬ 
theless,  the  hierarchical  cell  decomposition  approach  presents  a  rather  unique  combi¬ 
nation  of  attractive  features.  It  is  relatively  easy  to  implement  even  when  the  moving 
object  can  both  translate  and  rotate.  It  is  reasonably  efficient  when  the  number  of 
degrees  of  freedom  of  the  moving  object  is  small,  and  parallelization  is  possible  for 
achieving  better  performances.  It  is  complete  under  reasonable  assumptions,  if  we 
accept  no  upper  bound  on  the  worst-case  time  complexity,  and  “resolution-complete” 
otherwise.  Finally,  as  mentioned  above,  it  produces  a  channel,  which  leaves  some 
freedom  at  execution  time,  for  instance,  to  optimize  dynamic  behavior  and  avoid 
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unexpected  obstacles. 

However,  despite  the  conceptual  simplicity  of  the  main  two  steps  of  the  approach  -  cell 
decomposition  and  graph  searching  their  efficient  implementation  raises  delicate 
questions  not  thoroughly  addressed  in  previous  publications.  While  we  were  imple¬ 
menting  and  experimenting  with  a  planner  for  a  mobile  robot,  we  found  out  that 
efficiency  can  be  sharply  increased  by  shifting  from  naive  answers  to  these  questions, 
to  more  sophisticated  ones.  In  this  paper,  we  present  in  detail  the  new  algorithms 
which  we  have  developed,  and  we  report  on  our  experimentation  with  the  implemented 
planner. 

The  problem  of  decomposing  a  MIXED  cell  is  to  maximize  the  volume  of  the  EMPTY 
and  FULL  cells  resulting  from  the  decomposition,  in  order  to  make  it  possible  find¬ 
ing  a  path  (or  the  absence  of  path)  as  quickly  as  possible.  In  particular,  the  blind 
2n-tree  (e.g.,  quadtree,  octree)  decomposition  technique  has  the  drawback  of  decom¬ 
posing  many  MIXED  cells  into  smaller  MIXED  cells.  We  propose  a  new  “constraint 
reformulation”  technique,  which  provides  better  results  than  earlier  decomposition 
techniques.  It  consists  of  approximating  the  obstacles  intersecting  with  the  MIXED 
cell  to  be  decomposed  by  a  collection  of  rectangloids,  and  computing  the  complement 
of  these  rectangloids  in  the  cell.  Two  types  of  approximation  are  used,  “bounding” 
and  “bounded”  approximations.  The  first  is  used  to  produce  EMPTY  cells,  the  sec¬ 
ond  to  produce  FULL  cells. 

The  problem  of  graph  searching  is  to  take  advantage  of  unsuccessful  search  work 
done  at  lower  levels  of  approximation,  since  most  of  the  search  graph  remains  the 
same  from  one  level  to  the  next  (only  the  portions  of  the  graph,  which  correspond 
to  decomposed  MIXED  cells,  are  modified).  We  propose  a  set  of  search  techniques 
based  on  appropriate  representation  of  the  search  graph  and  on  the  recording  of 
failure  conditions.  These  techniques,  inspired  from  those  implemented  in  dependency- 
directed  backtracking  systems  [Stallman  and  Sussman,  1977]  [Latombe,  1979],  avoid 
the  path  planner  to  run  into  the  same  mistakes  several  times. 

The  paper  consists  of  four  sections,  in  addition  to  the  introduction  and  the  conclusion. 
In  Section  2  we  review  the  concepts  underlying  the  hierarchical  cell  decomposition  ap¬ 
proach  and  introduces  the  necessary  terminology.  In  Sections  3  ana  4,  we  investigate 
the  cell  decomposition  and  graph  searching  problems,  respectively,  and  we  describe 
in  detail  the  new  algorithms  which  we  have  developed.  In  Section  5  we  present  some 
of  the  experiments  which  we  conducted  with  the  implemented  planner. 
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2  Background  and  Overview 

2.1  Configuration  space 

Let  us  consider  a  rigid  object  A  moving  in  a  Euclidean  space  W  =  R^  -  called  the 
workspace  -  among  fixed  obstacles  0,-,  i  =  1,...,<j.  Both  A  and  the  S.’s  are  closed 
regions  in  R" 

A  standard.  Cartesian  coordinate  frame,  denoted  by  !Fw,  is  embedded  in  W.  Another 
Cartesian  frame,  denoted  by  Ta-,  is  embedded  in  A.  The  origin  of  Ta,  denoted  by 
0, 4,  is  called  the  reference  point  of  A. 

A  configuration  of  A  is  a  specification  of  the  position  and  orientation  of  Ta  with 
respect  to  Tyj.  The  configuration  space  of  A  is  the  space,  denoted  by  C,  of  all  the 
possible  configurations  of  A.  The  unique  configuration  where  Ta  and  Tw  coincide 
is  called  the  reference  configuration  of  A.  The  subset  of  W  occupied  by  A  at 
configuration  q  is  denoted  by  A(q). 

The  obstacles  0,’s  map  in  C  as  closed  regions  denoted  by  C0,  and  called  C-obstacles. 
These  regions  axe  defined  by: 

CBi  =  {q  €  C  /  A{ q)  n  0,  f  0}. 


The  region: 

Cfree=C~  (J  CBi 

«'e[i ,?] 

is  called  the  free  space.  A  collision-free  path  (more  simply,  a  free  path)  is  any 
continuous  map  r  :  [0,1]  — »  £/ree- 

In  this  paper,  we  consider  the  case  where  A  is  a  two-dimensional  object,  which  trans¬ 
lates  and  rotates  in  W  =  R2.  In  this  case,  C  is  a  manifold  diffeomorphic  to  R2  x  51, 
where  S 1  denotes  the  unit  circle  [Spivak,  1979].  We  parameterize  a  configuration  in 
this  manifold  by  a  triplet  (x,  y,  0),  where  x,y  €  R2  are  the  coordinates  of  Oa  in  Tw, 
and  0  (E  [0,27r)  is  the  angle  (modulo  27t)  between  the  x  axes  of  Tw  and  Ta- 

2.2  C-obstacles  in  Polygonal  Case 

Throughout  the  paper,  both  A  and  the  0,’s  are  polygons.  Under  this  condition, 
the  cross-section  of  each  CBi  at  any  orientation  0  is  also  a  polygon  defined  by  (see 
[Lozano- Perez,  1983]): 

CB, ,  =  0,  9  .4(0, 0,0) 

where  >1(0, 0,0)  denotes  the  region  occupied  by  A  at  configuration  (0,0, 0)  and  ©  is 
the  symbol  for  the  Minkowski  difference.  Each  edge  of  this  polygon  is  the  locus  of  Oa 
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Figure  1.  Both  A  and  B  are  polygons.  The  cross-section  through  the  C-obstacle  CB  at  a 
fixed  orientation  9  of  A  is  another  polygon  equal  to  B  Q  .4(0, 0, 9)  (see  text). _ 

when  A  translates  at  fixed  orientation  &,  in  such  a  way  that  an  edge  (resp.  a  vertex) 
of  A  stays  in  contact  with  a  vertex  (resp.  an  edge)  of  Z?,-  (see  Figure  1).  A  contact 
between  an  edge  of  A  and  a  vertex  of  is  called  a  Type  A  contact.  When  A  rotates 
slightly,  the  corresponding  edge  of  CBi  rotates  by  the  same  angle.  A  contact  between 
a  vertex  of  A  and  an  edge  of  is  called  a  Type  B  contact.  When  A  rotates  slightly, 
the  corresponding  edge  of  CBi  translates. 

Therefore,  the  C-obstacle  CB  corresponding  to  an  obstacle  B  is  a  three-dimensional 
volume  without  hole,  which  is  bounded  by  patches  of  ruled  surfaces,  which  we  call 
C-facets.  Each  C-facet  is  generated  by  a  straight  line  segment  of  variable  length, 
which  remains  parallel  to  the  xy  plane  and  either  translates  or  rotates.  A  C-facet 
created  by  a  contact  of  Type  A  (resp.  Type  B)  is  called  a  Type  A  (resp.  Type  B) 
C-facet.  Each  C-facet  is  comprised  between  two  limit  orientations,  beyond  which  the 
contact  that  creates  the  C-facet  is  no  longer  feasible. 

The  geometry  of  CB,  when  A  and  B  are  polygons,  is  studied  in  depth  in  various 
publications,  e.g.  [Lozano- Perez,  1983]  [Donald,  1984]  [Avnaimand  Boissonnat,  1988] 
[Brost,  1 9S9] . 

If  A  and  B  are  both  convex  polygons,  CB  is  bounded  by  O(n^no)  C-facets,  where 


and  ng  axe  the  number  of  edges  in  A  and  B,  respectively.  If  A  and  B  are  non-convex, 
CB  has  f2(n^n|)  C-facets  [Avnaim  and  Boissonnat,  1988]. 


2.3  Rectangloid  Decomposition 

In  the  following,  we  will  assume,  without  practical  loss  of  generality,  that  the  range 
of  possible  values  for  x  and  y  are  closed  intervals  [xmtn,xmax]  and  [ymin,t/mai]-  We 
represent  C  as  a  closed  rectangloid 

K,  —  [xmtn,  xmax]  x  [3/mtn?  ymax]  X  [0,2*]  C  R3 

with  the  two  cross-sections  at  9  =  0  and  9  =  2x  procedurally  identified. 

Let  k  be  a  rectangloid,  i.e.  a  region  of  the  form: 

[xi,x2]  X  [t/1 ,  2/2]  X  [01,02]  Q  £• 

A  rectangloid  decomposition  of  k  is  a  collection  of  rectangloids,  {Kj}J=1 . n,  such 

that: 

1.  /c  is  equal  to  the  union  of  the  Kj,  i.e.: 

n 

«  =  u  Kr 
j'=i 


2.  The  Kj  are  non-overlapping1: 

Vji,j2  €  [l,n],  ii  ^  j2  :  int(Kh)  D int(Kh)  =  0. 

Each  rectangloid  Kj  is  called  a  cell  of  k  in  the  decomposition.  Two  cells  /c;i  and 
/Cj,  are  adjacent  iff  their  intersection  is  not  a  set  of  measure  zero  in  R2,  i.e.  the 
intersection  of  two  of  their  faces  is  a  surface  of  non-zero  area.  (The  intersection  is 
computed  by  taking  into  account  that  the  cross-sections  at  0  and  are  identified.) 

A  cell  Kj  is  classified  as: 

-  EMPTY,  iff  its  interior  int(Kj)  intersects  no  C-obstacle,  i.e.  int(Kj)  fl{J ,  =  0; 

-  FULL,  iff  Kj  is  entirely  contained  in  the  union  of  the  C-obstacles,  i.e.  Kj  C  U,  CB{\ 

-  MIXED,  otherwise. 

Given  an  initial  configuration 

Qinil  =  (•Einiti  2/inity  9{nit) 

'Throughout  this  paper,  we  say  that  two  closed  sets  are  non-overlapping  if?  their  interiors  do  not 
intersect. 
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and  a  goal  configuration 


Qgoal  —  (“Cgoah  Vgoa  I  >  ®goal ) ; 


a  rectangloid  decomposition  of  £,  . n>  is  admissible  iff  it  contains  a  sequence 

of  EMPTY  cells  /cjt,  k  =  l,...,p,  called  an  EMPTY  channel,  such  that: 


‘  {xiniti  1/iniU  @init)  €  *1, 

*  {.% goahy goal goal)  £ 

-  Vk  6  [l,p  —  1]  :  «it  and  ick+\  are  adjacent. 

Let  (K*)fc=i  P  be  an  EMPTY  channel.  Any  path  connecting  y,mt,  6tntt)  to 

{Xgoah  Vgoah  9goat)  and  lying  entirely  in  the  interior  of  U£=l  Kk  is  a  free  path.  Such  a 
path  can  easily  be  constructed  as  an  open  polygonal  line.  If  necessary,  it  can  also  be 
smoothed  by  fitting  splines  curves  [Kant  and  Zucker,  1986). 

A  rectangloid  decomposition  is  said  to  be  semi-admissible  if  it  contains  a  sequence 
of  EMPTY  and  MIXED  cells  nk ,  k  —  1,  ...,p,  called  MIXED  channel,  with  the  same 
three  properties  as  above.  A  MIXED  channel  may  contain  a  free  path  connecting  the 
initial  to  the  goal  configuration,  but  there  is  no  guarantee  that  this  is  the  case. 

One  could  think  of  decomposing  configuration  space  into  fine  cells  at  a  single  level 
of  approximation,  labelling  the  cells  EMPTY  of  FULL  depending  on  whether  their 
interiors  lie  entirely  in  free  space  or  not,  and  searching  the  connectivity  graph  of 
the  EMPTY  cell  (see  [Gouzenes,  1984]  [Lozano-Perez,  1987]).  The  problem  with 
this  one-level  decomposition  approach  is  that  either  the  number  of  cells  is  large  and 
the  planner  is  always  time  inefficient,  or  the  number  of  cells  is  relatively  small  (i.e., 
the  decomposition  is  coarse)  and  the  planner  often  fails  to  find  free  paths,  while 
some  exist.  The  motivation  for  the  hierarchical  decomposition  introduced  below  is 
that  in  general  large  chunks  of  configuration  space  may  be  labelled  EMPTY  or  FULL, 
drastically  reducing  the  number  of  cells  and  consequently  the  size  of  the  search  graph. 


2.4  Hierarchical  Path  Planning 

Hierarchical  path  planning  consists  of  generating  an  EMPTY  channel  by  constructing 
successive  rectangloid  decompositions  of  fC,  until  an  admissible  decomposition  has 
been  generated  and  an  EMPTY  channel  has  been  extracted.  Let  Vt,  l  =  0,1,..., 
denote  the  successive  decompositions  of  fC,  with  Vo  =  {£}.  Each  decomposition  Vi, 
l  >  0,  is  obtained  from  the  previous  one  P/_i,  by  decomposing  one  or  several  MIXED 
cells,  the  other  cells  being  unchanged. 

Whenever  a  decomposition  Vi,  /  >  0,  is  generated  and  its  cells  labelled  EMPTY, 
FULL,  or  MIXED,  an  undirected  graph  denoted  by  CCQi  is  constructed: 

-  The  nodes  of  CCQi  are  the  EMPTY  and  MIXED  cells  in  Vi. 
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-  Any  two  nodes  are  connected  by  a  link  iff  they  are  adjacent. 

This  graph  is  called  the  cell-connectivity  graph,  or  ccg,  of  the  decomposition 

Once  constructed2,  the  graph  CCQi  is  searched  for  an  EMPTY  or  MIXED  channel. 
Three  outcomes  are  possible: 

1.  An  EMPTY  channel  is  found.  Then,  the  planner  returns  success. 

2.  No  EMPTY  or  MIXED  channel  is  found.  Then,  the  planner  returns  failure. 

3.  A  MIXED  channel  is  found,  but  no  EMPTY  channel. 

In  the  third  case,  the  planner  proceeds  recursively  by  decomposing  the  MIXED  cells 
contained  in  the  MIXED  channel.  Hence,  the  planner  iteratively  refines  the  “inter¬ 
esting”  areas. 

Let  us  assume  that  the  region  occupied  by  the  C-obstacles  can  be  expressed  as  the 
union  of  disjoint  sub-regions,  each  equal  to  the  closure  of  its  interior  ( “full-bodiness 
assumption”)3.  Then,  the  planning  process  sketched  above  can  be  made  complete  - 
i.e.,  guaranteed  to  terminate  successfully,  whenever  a  free  path  exists,  and  to  return 
failure,  otherwise.  This  requires  that  some  details  of  the  algorithms  be  worked  out 
appropriately.  However,  for  an  unknown  problem,  there  is  no  time  bound  on  the 
process,  since  there  is  no  lower  bound  on  the  size  of  the  cells  which  may  have  to  be 
generated. 

The  worst-case  time  complexity  can  be  bounded  at  the  expense  of  completeness,  by 
imposing  constraints  on  the  decomposition  of  any  MIXED  cell  k.  One  possible  con¬ 
straint  is  that  the  total  volume  of  the  EMPTY  and  FULL  cells  extracted  from  k  be 
greater  than  a  predefined  ratio  of  the  volume  of  k;  in  addition,  every  MIXED  cell 
resulting  from  the  decomposition  of  k  which  has  any  of  its  dimensions  (side  length) 
smaller  than  a  predefined  value  is  relabelled  FULL.  Another  possible  constraint  is 
that  every  generated  cells  should  have  dimensions  greater  than  prespecified  values. 
The  choice  of  the  constraints  on  the  decomposition  of  a  MIXED  cell  typically  de¬ 
pends  on  the  decomposition  technique.  If  the  decomposition  algorithm  fails  to  find 
a  decomposition  of  k  satisfying  the  constraint,  k  is  said  to  be  non-decomposable  and 
is  re-labelled  FULL. 

If  constraints  are  imposed  on  MIXED  cell  decomposition,  it  is  no  longer  guaranteed 
that  a  free  path  is  found,  whenever  one  such  path  exists.  However,  the  planner  can 
be  made  “resolution-complete”,  i.e.  if  an  EMPTY  channel  exists  at  the  resolution 
determined  by  the  constraints,  it  will  be  found. 

2  Actually,  the  graph  is  a  by-product  of  the  decomposition  and  is  constructed  concurrently  with 
the  decomposition. 

3In  Solid  Modelling,  a  closed  set  that  is  the  closure  of  its  interior  is  called  a  regular  set. 
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If  the  assumption  of  “full-bodiness”  of  the  disjoint  C-obstacIe  regions  is  not  satisfied, 
then  in  the  absence  of  constraints  on  the  decomposition  of  MIXED  cells,  the  planning 
process  may  loop  for  ever.  The  constraints  given  above  guarantee  termination  in 
bounded  time. 

In  the  presence  of  uncertainty  in  robot  control,  a  minimal  size  requirement  may  also 
have  to  be  imposed  to  both  the  cells  in  a  channel  and  the  intersection  of  two  successive 
cells  in  the  channel,  in  order  to  allow  the  robot  to  move  safely  despite  uncertainty. 


3  Cell  Decomposition 

3.1  Issue 

The  subproblem  considered  in  this  section  is  that  of  generating  a  rectangloid  de¬ 
composition  {Kj}J=1 . n  of  a  given  MIXED  cell  k.  For  the  efficiency  of  the  planning 

process,  the  generated  decomposition  should  simultaneously  satisfy  the  following  two 
goals: 

1.  The  number  of  cells  in  the  decomposition  should  be  reasonably  small,  in  order 
to  keep  the  size  of  the  search  graph  as  tractable  as  possible.  This  goal  directly 
relates  to  the  motivation  for  the  overall  hierarchical  strategy:  we  want  the  plannei 
to  consider  “details”  only  when  it  is  necessary. 

2.  The  volume  of  the  EMPTY  and  FULL  cells  should  be  large  relatively  to  the  total 
volume  of  k.  This  goal  is  aimed  at  reducing  as  quickly  as  possible  the  “uncertain 
area”  -  i.e.,  the  MIXED  cells.  Clearly,  decomposing  a  MIXED  cell  into  smaller 
cells  would  not  be  useful,  if  all  of  these  cell  were  MIXED. 

These  two  goals  may  be  conflicting  since  one  obvious  way  to  achieve  the  second  goal 
is  to  produce  many  small  cells,  which  is  in  contradiction  with  the  first  one.  A  good 
quantitative  measure  of  the  efficiency  of  the  decomposition  of  a  cell  may  be: 

£  _  _ 1 _ V EMPTY  +  VfULL 

NemPTY  +  N MIX  ED  Kc 

where: 

-  A empty  (resp.  Nmixed)  is  the  number  of  newly  generated  cells  labelled  EMPTY 
(resp.  MIXED), 

-  Vempty  (resp.  Vfull)  *s  the  total  volume  of  newly  generated  EMPTY  (resp. 
FULL)  cells, 

-  VK  is  the  volume  of  the  cell  k. 
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3.2  Previous  Approaches 

One  simple  method  of  decomposing  a  MIXED  cell  is  to  partition  it  into  2m  cells  of 
equal  dimensions,  where  m  is  the  dimension  of  configuration  space  (3  in  our  case). 
The  overall  decomposition  of  configuration  space  obtained  with  this  method  is  called 
“quadtree”  when  m  —  2  and  “octree”  when  m  =  3  [Ayala  et  al,  1985].  The  appli¬ 
cation  of  this  method  is  reported  in  [Kambhampati  and  Davis,  1986]  (quadtree)  and 
[Faverjon,  1986]  (octree).  The  advantage  of  this  method  is  that  it  leads  to  a  decom¬ 
position  easily  representable  in  a  tree  structure  of  degree  2m.  The  drawback  is  that 
most  of  the  time  none  or  a  few  of  the  newly  generated  cells  are  EMPTY  or  FULL. 
The  method  tends  to  produce  a  huge  number  of  cells. 

Another  method  is  described  in  [Brooks  and  Lozano-Perez,  1983].  The  basic  idea  is 
to  consider  potential  cuts  of  the  cell,  score  them,  and  choose  the  best.  The  potential 
cuts  <ire  chosen  wherever  a  C-surface4  will  go  through  a  vertex  of  one  of  the  new 
cells  generated  by  the  decomposition.  The  scoring  function  favors  cuts  which  do 
not  generate  small  cells,  i.e.  the  cuts  closer  from  the  mid-points  of  each  edge  are 
preferred.  The  scoring  function  also  attempts  to  minimize  the  number  of  C-surfaces 
intersected  by  each  new  cell,  in  order  to  reduce  future  computations.  This  method  has 
the  drawback  of  treating  each  C-surface  (not  C-facets)  individually  and  to  combine 
the  effect  of  the  various  C-surfaces  in  a  global  scoring  function.  Although  certainly 
better  than  a  2"*-tree,  the  resulting  decomposition  may  still  be  fax  from  optimal.  It 
may  also  incorrectly5  label  a  cell  that  intersects  no  C-obstacle  as  MIXED. 

In  [Lozano-Perez,  1983],  Lozano-Perez  describes  a  method  which  consists  of  slicing 
the  orientation  axis  into  intervals,  computing  the  area  swept  out  by  A  when  it  rotates 
about  its  reference  point  in  each  interval,  approximating  the  swept  area  as  a  polygon, 
and  growing  the  obstacles  by  this  polygon.  The  result  is  a  decomposition  of  config¬ 
uration  space  into  prismatic  cells,  which  are  either  empty  or  not  empty  (the  latter 
axe  not  characterized  further).  A  path  is  search  among  the  empty  cells  only.  If  it 
fails,  the  angular  intervals  are  refined.  The  decomposition  method  is  very  similar  to 
the  “swept-area”  method  proposed  below.  Our  method,  however,  generates  EMPTY, 
FULL  and  MIXED  cells. 

3.3  Constraint  Reformulation 

We  propose  a  different  approach  for  decomposing  MIXED  cells.  It  consists  of  first 
approximating  each  C-obstacle  lying  in  the  cell  k  to  be  decomposed  as  a  collection 
of  non-overlapping  rectangloids.  The  complement  of  a  union  of  rectangloids  within 
a  rectangloid  region  is  also  a  union  of  rectangloids,  which  can  easily  be  computed. 

4  A  C-surface  is  the  infinite  ruled  surface  that  support  a  C-facet. 

'’This  conservative  “error”  gets  corrected  when  the  decomposition  proceeds  deeper. 
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Figure  2.  This  figure  illustrates  the  notion  of  bounding  (a)  and  bounded  (b)  approxima¬ 
tions  of  a  region  in  a  two-dimensional  space. _ 


This  yields  a  rectangloid  decomposition  of  k.  We  call  this  approach  contraint  refor¬ 
mulation,  since  it  basically  consists  of  reformulating  the  constraints  imposed  by  the 
C-obstacles  into  a  form  directly  compatible  with  the  format  of  the  decomposition  of 
K,  into  rectangloid  cells. 

Let  k  —  [xi,i2]  x  [yi, 2/2]  x  [$i, 02]  he  the  MIXED  cell  to  be  decomposed.  For  every 
i  €  [l,?],  we  denote  by  CBi{k\  the  portion  of  CB,  contained  in  k,  i.e.: 

CBi[K)  =  CBi  n  k. 

Our  planner  generates  and  uses  two  types  of  approximation  of  C-obstacles,  bounding 
and  bounded  approximations: 

1.  A  bounding  approximation  of  CBi[i c]  is  a  collection  of  non-overlapping  rectan- 
gloids  7 Zik's,  k  =  1  ,...,p,  with  Vk  £  [l,p]  :  ft,*  C  «  and  C5,[/c]  C  U*=i,...,p  ft,*- 

2.  A  bounded  approximation  of  CBi[t c]  is  a  collection  of  non-overlapping  rectan- 

gloids  ft'*’s,  k  =  1 with  (J*=i . P-  Kk  ^  CB^k). 

The  EMPTY  cells  of  the  decomposition  of  «;  are  obtained  by  computing  the  comple¬ 
ment  of  U,  U*  ft,*  in  «.  The  FULL  cells  are  the  ft/*’ s.  The  MIXED  cells  are  obtained 
by  computing  the  complement  of  UiU*  ^*  in  every  ft,*. 

Figure  2  illustrates  the  notion  of  bounding  and  bounded  approximations  in  a  two- 
dimensional  space.  Figure  3  illustrates  the  rectangloid  decomposition  of  a  cell  k  into 
EMPTY,  FULL  and  MIXED  cells,  using  these  two  approximations. 
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Figure  3.  This  figure  illustrates  the  decomposition  of  a  rectangular  cell  into  a  collection 
of  EMPTY,  FULL  and  MIXED  cells  built  using  the  bounding  and  bounded  approximations 
of  Figure  2. _ 


3.4  Outline  of  the  Algorithm 

There  are  infinitely  many  ways  to  generate  bounding  and  bounded  approximations  of 
a  C-obstacle  CB  in  a  cell  k.  Our  method  computes  “outer”  and  “inner”  projections 

of  CB  on  the  xy  plane,  next  on  the  x  or  y  axis.  Then,  it  lifts  back  the  projection  into 

rectangloid  cells.  It  consists  of  the  following  two  steps: 

1.  Decomposition  of  [Bt,  02):  The  [0U  02]  interval  is  cut  into  non-overlapping  subin¬ 
tervals  [7u,7«+i],  u  —  1,  ...,r,  r  >  1,  with  71  =  V\  and  7r+1  =  02 .  We  denote  by  /c“ 
the  rectangloid  [xx,x2]  x  [yi,y2]  X  [7„,7u+i]- 

For  every  tt  €  [l,r],  we  compute  the  outer  projection  and  the  inner  projection 
of  CB[ku]  on  the  xy  plane.  These  two  projections,  which  we  respectively  denote 
by  OCBxj/[ku ]  and  Z CJBxy[Ku]t  are  defined  by: 

OCBxy[Ku]  =  {(x,y)  /  3 0  £  [7u,7u+iJ  :  (x,y,&)  €  CB[ku]}, 

ICBxy[nu]  =  {(x,y)  /  W  €  [7«t7u+i]  :  (x,y,0)  eCB[Ku]}. 

Clearly,  we  have:  TCBxy[Ku]  C  OCBxy[Ku]. 

2.  Decomposition  of  [xi^]  and  [yi,yj]:  For  every  u  €  [1 , r],  the  interval  [x!,x2J 
or  [U\  >2/2])  whichever  is  longer,  is  cut  into  non-overlapping  subintervals. 

Let  us  assume  that  [xx,x2\  is  subdivided  (a  similar  presentation  would  be  made  in 
the  case  where  (yi,y2|  was  decomposed).  The  generated  subintervals  are  [av,av+j], 
v  =  1 s,  s  >  1  with  a)  =  xx  and  as+l  =  x2.  We  denote  by  kuv  the  rectangloid 
Ki av+ij  x  [y,,y2]  x  [7u,7«+i]- 


12 


For  every  v  6  [1,5],  we  compute  the  outer  projection  of  OCBxy[f cu]  on  the  y  axis, 
i.e.: 

OCBy[K™]  =  {y  t  3xe  K,a„+i] :  (x,y)  6  OCB^k")} 
and  the  inner  projection  of  X CBXy[nu]  on  the  y  axis,  i.e.: 

ICBy[> c“w]  =  {y  /  Vx  6  [av,av+1]  :  (z,y)  €  XCB ■*,[«*]}. 

Both  OCBv(kuv]  and  XCBv[kuv\  are  sets  of  intervals.  XCBy[KuvJ  C  OCBv[kuv). 

Each  rectangloid  [av,  a„+1]  x  [b,V]  x  [7U,  7u+i],  where  [6,6']  €  OCBy[K'iV]  (resp. 
XCBy[icuv])  is  a  rectangloid  IZik  (resp.  TZ'ik)  in  the  bounding  (resp.  bounded)  ap¬ 
proximation  of  CBi[i c]  (see  Subsection  3.3). 

The  choice  of  the  7„’s  and  the  a„’s  is  empirical.  Vaxious  heuristics  can  be  used,  but 
most  of  them  seem  to  have  limited  effect  on  the  average  efficiency  of  the  method.  The 
only  useful  heuristic  guideline  is  to  keep  the  three  dimensions  of  every  MIXED  cell 
approximately  “homogeneous” .  Let  Sx,  Sy  and  SO  be  these  dimensions.  We  say  they 
are  “homogeneous”  iff  Sx  &  Sy  m  pS0,  where  p  is  the  maximal  distance  between  Oa 
and  the  points  in  the  boundary  of  A. 

In  the  next  two  subsections,  we  describe  the  computation  of  the  outer  and  inner 
projections  of  CB[ku]  on  the  xy  plane.  We  propose  two  different  methods.  The  first, 
called  “projection”  method,  consists  of  computing  the  projection  of  the  surface  of  CB 
comprised  between  7„  and  7„+i,  and  clipping  the  subset  of  the  projection  contained 
in  [ij, x2]  X  [j/i, y2]-  The  second  method,  called  “swept-area”  method,  consists  of 
computing  the  “outer”  and  the  “inner”  swept  areas  of  A  when  it  rotates  around  the 
reference  point  from  orientation  -fu  to  orientation  7u+i>  and  growing  the  obstacle  B 
by  these  two  areas.  The  projection  method  is  preferred  when  the  interval  [7u,7u+i] 
is  small.  The  swept-area  method  runs  significantly  faster  when  this  interval  is  large; 
however,  it  does  not  exactly  compute  X CBxv[ku],  but  a  subset  of  it.  The  computation 
of  OCBv[/c"”]  and  X CBV[«UW]  is  quite  simple  and  not  described  below. 

In  the  following,  we  assume  that  the  reference  point  Oa  is  chosen  in  the  interior  of  A. 
In  fact,  the  choice  of  Oa  has  some  impact  on  the  efficiency  of  the  decomposition.  The 
“best”  location  of  Oa  should  minimize  the  area  of  the  outer  projection  of  CB[> cu]  on 
the  xy  plane,  while  maximizing  the  area  of  the  inner  projection,  so  that  less  MIXED 
cells  are  ultimately  generated.  The  center  of  the  “smallest  enclosing  circle”  of  A 
is  probably  close  to  minimize  the  area  of  the  outer  projection,  while  the  center  of 
the  “largest  enclosed  circle”  of  A  should  be  close  to  maximize  the  area  of  the  inner 
projection.  A  compromise  between  these  two  locations  is  in  general  necessary,  since 
they  do  not  coincide. 
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Figure  4.  The  C-patches  comprised  in  an  angular  slice  [7U.  7«+i]  project  on  the  xy  plane 
according  to  generalized  polygons,  whose  union  is  a  “donut”  shaped  region  bounded  by  an 
outer  contour  Ti  and  an  inner  contour  IV 

3.5  Projection  Method 

Principle.  Let  us  first  assume  that  both  A  and  B  axe  convex  polygons.  In  R2  x 
[0,2*],  CB  is  a  volume  without  hole,  which  is  bounded  by  C-facets  (see  Subsection 
2.2).  Consider  the  point  (x0,t/o)  in  the  xy  plane  and  the  segment  {(xO5yo>0)  /  0  € 
[7u)7u+i]}  above  this  point.  If  the  segment  pierces  a  C-facet,  then  (x0, yo)  is  in 
OCBxy[Ku],  but  not  in  TCBxy[Ku].  If  it  pierces  no  C-facet,  then  either  (x0,y0)  is  not 
in  OCBxy[f c“],  or  it  is  in  ZCBxy[Ku].  The  segment  {(xo,yo,0)  /  9  £  [7u,7«+i]}  pierces 
a  C-facet  e  iff  (x0,yo)  lies  in  the  projection  of  e  in  the  xy  plane. 

We  show  below  that  each  C-facet  comprised  between  7„  and  7„+1  projects  on  the  xy 
plane  according  to  a  generalized  polygon6.  The  projection  of  the  boundary  of  CB  that 
is  comprised  between  7*  and  7 U+J  is  the  union  of  generalized  polygons,  each  being 
the  projection  of  a  C-facet.  This  union  is  a  “donut”  shaped  region  (see  Figure  4), 

SA  generalized  polygon  is  a  compact  two-dimensional  region  bounded  by  a  simple  curve  consisting 
of  straight  segments  and  circular  arcs. 


with  an  outer  boundary  Tj  and  an  inner  boundary  r2.  The  compact  region  bounded 
by  rt  is  OCBxy[Ku\.  The  compact  region  bounded  by  T2  is  XCBxy\> cu]. 

Therefore,  the  projection  method  consists  of:  (1)  projecting  all  the  C-facets  (to  the 
extent  they  are  contained  in  the  interval  [7u,7u+i])  on  the  xy  plane;  (2)  clipping  the 
union  of  the  projections  by  the  rectangle  [xi,x2]  x  [irijlri]  and,  within  this  rectangle, 
tracking  Ti  and  T2. 

The  projection  method  may  be  made  faster  by  associating  with  each  interval  [7„,7u+i] 
the  list  of  all  the  C-obstacles  having  a  non-empty  intersection  with  the  interval,  and 
for  each  of  these  C-obstacles  the  list  of  the  C-facets  that  intersect  the  interval.  If  the 
interval  gets  decomposed  further,  only  these  C-facets  have  to  be  considered. 

The  projection  method  is  efficient  when  the  number  of  C-facets  which  have  to  be 
projected  is  reasonably  small,  that  is  when  the  interval  [7u,7u+i]  is  small. 

Projection  of  Type  A  C- Facet.  Let  eA  be  a  C-facet  of  Type  A  comprised  between 
the  limit  orientations  fa  and  fa.  The  contact  that  generates  eA  is  illustrated  in  Figure 
5  a.  It  occurs  between  an  edge  E  of  A  and  a  vertex  b  of  B.  E  connects  two  vertices 
of  A ,  ax  and  a2.  b  is  the  extremity  of  two  edges  of  B,  Fx  and  F2.  0A  projects  on  the 
supporting  line  of  E  at  the  point  p.  We  assume  below  that  p  is  located  between  a2 
and  a2.  (The  case  where  p  is  outside  the  segment  is  treated  in  a  very  similar 
fashion.)  The  orientation  fa  (resp.  fa)  is  achieved  when  the  edge  E  is  aligned  with 
the  edge  Ft  (resp.  F2).  Assuming  that  [^1,^2]  C  [7„,7u+i],  the  projection  of  eA  on 
the  xy  plane  is  shown  in  Figure  5  d.  It  is  obtained  as  the  union  of  two  regions  shown 
in  Figures  5  b  and  c. 

The  region  in  Figure  5  b  is  the  locus  of  0A,  when  A  translates  and  rotates  while 
the  edge  segment  ajp  stays  in  contact  with  the  vertex  b.  This  region  is  bounded  by 
two  circular  arcs  and  two  straight  segments.  The  two  arcs  are  centered  at  b.  The 
smaller  one  is  the  locus  of  0A  when  p  coincides  with  b  and  A  rotates  from  fa  to  fa. 
The  larger  arc  is  the  locus  of  0A  when  a2  coincides  with  6  and  A  rotates  from  <f>\  to 
fa.  The  straight  segments  are  the  loci  of  0A  when  A  translates  at  fixed  orientations 
4>  1  and  fa  from  the  position  where  p  and  b  coincide  to  the  position  where  a2  and  b 
coincide.  The  region  in  Figure  5  c  is  the  locus  of  0A ,  when  A  translates  and  rotates 
while  the  edge  segment  pai  stays  in  contact  with  the  vertex  b. 

The  case  of  eA  projecting  for  the  orientation  range  [7u,7u+i]  C  [fa,  fa]  can  be  treated 
in  the  same  way,  by  drawing  fictitious  edges  F2  and  F2  from  vertex  b  (see  Figure  5 
e),  so  that,  when  E  is  aligned  with  F2  (resp.  F2),  A’s  orientation  is  7U  (resp.  7u+i). 

Projection  of  a  Type  B  C-Facet.  Let  es  be  a  C-facet  of  Type  B  comprised 
between  the  limit  orientations  (f>i  and  fa.  The  contact  that  generates  eg  is  illustrated 
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in  Figure  6  a.  It  occurs  between  a  vertex  a  of  A  and  an  edge  F  of  B.  a  is  the  extremity 
of  two  edges  of  A,  E\  and  E2.  F  connects  two  vertices  of  B,  b\  and  b 2.  Since  we 
assumed  A  to  be  convex,  Oa  lies  within  the  convex  angular  sector  bounded  by  the 
two  half-lines  supporting  E\  and  E2  and  erected  from  a.  The  orientation  <pi  (resp. 
<f> 2 )  is  achieved  when  the  edge  E2  (resp.  E\)  is  aligned  with  the  edge  F .  Assuming 
that  (<£1,^2]  Q  frmTu+i],  the  projection  of  cb  on  the  xy  plane  is  shown  in  Figure  6  d. 
It  is  obtained  as  the  union  of  two  regions,  shown  in  Figures  6  b  and  c. 

Let  0  be  the  orientation  of  A  when  a  lies  in  F  and  the  segment  aO a  is  perpendicular 
to  F.  The  region  in  Figure  6  b  is  the  locus  of  O^,  when  A  translates  and  rotates  with 
the  vertex  a  staying  in  F  and  the  orientation  0  ranging  over  [<f> i,0].  (If  0  <  0lt  then 
the  region  is  empty.)  This  region  is  bounded  by  two  circular  arcs  and  two  straight 
segments.  The  two  arcs  are  centered  at  bx  and  62,  respectively,  and  have  the  same 
radius  equal  to  the  distance  between  a  and  Oa-  The  region  in  Figure  6  c  is  the 
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Figure  6.  This  figure  illustrates  the  computation  of  the  projection  of  a  C-patch  of  Type 
B  on  the  xy  plane  (see  text). _ 


locus  of  0^,  when  A  translates  and  rotates  with  a  staying  in  F  and  the  orientation 
9  ranging  over  [0,  4>2]-  (If  >  <f>2,  the  region  is  empty.) 

The  case  of  zb  projecting  for  the  orientation  range  [7u,7„+i]  C  [<^i,  <f>2]  can  be  treated 
in  the  same  way,  by  drawing  fictitious  edges  Ex  and  E2  from  vertex  a  (see  Figure  6 
e  and  f),  so  that,  when  E\  (resp  E2)  is  aligned  with  F,  .A’s  orientation  is  -yu  (resp. 
7u+i)- 


Computation  of  OCBxy[nu ]  and  XCBxy[nu].  The  projection  of  every  C-facet  is  a 
generalized  polygon  with  a  small  number  of  edges  -  two  to  four  straight  line  segments 
and  two  or  three  circular  arcs.  The  union  of  all  the  generalized  polygons  forms  a 
“donut”  shaped  region  (see  Figure  4). 

Tj  and  T2  can  be  extracted  by  a  line-sweep  technique.  This  is  a  well-known  tech¬ 
nique  in  Computational  Geometry  (e.g.,  see  [Preparata  and  Shamos,  1985]),  whose 
application  to  the  computation  of  the  intersections  of  arbitrary  generalized  polygons 
is  described  in  [Laumond,  1987].  It  consists  of  sweeping  a  line  across  the  plane.  For 
example,  the  line  is  parallel  to  the  x  axis  and  is  swept  bottom-up.  At  each  instant,  the 
“status”  of  the  line  -  i.e.,  the  list  of  the  intersections  of  the  line  with  the  generalized 
polygons  -  is  represented  in  a  balanced  tree  [Aho,  Hopcroft  and  Ullman,  1983].  The 
status  of  the  line  changes  in  a  qualitative  fashion  only  at  a  finite  number  of  ordinates, 
called  “events”,  where  the  line  is  either  tangent  to  a  C-obstacle  or  passing  through 
the  intersection  of  two  generalized  polygons.  At  every  event,  the  list  of  future  events, 
which  is  also  represented  in  a  balanced  tree,  is  updated. 

The  line-sweep  process  starts  at  the  bottom-most  ordinates  of  all  the  points  in  the 
generalized  polygons.  Below  this  ordinate,  the  line  intersects  no  generalized  polygon 
and  thus  lies  entirely  outside  OCBxy[Ku].  During  the  sweeping  process,  the  contours 
Tv  and  r2  are  tracked  by  labelling  the  intervals  between  the  ordinates  listed  in  the 
sweep-line  status  as  being  outside  OCBxy [«“],  inside  OCBxy[Ku\  but  outside  XCBxv[ku], 
or  inside  XCBxy[> cu]. 

Once  Ti  and  T2  have  been  extracted  in  the  form  of  sequences  of  straight  segments 
and  circular  arcs,  it  is  not  difficult  to  clip  them  by  the  rectangle  [x1;x2]  x  [j/i ,  y2].  The 
projections  OCBy[Kuv\  and  XCBy[Kuv ]  are  easily  computed  by  determining  the  points 
of  Ti  and  T2  at  abscissae  a„,  v  =  l,...,s  4- 1,  and  the  other  extremal  points  of  Tj  and 
r2  within  each  interval  [a,„av+1],  v  =  1,..., s.  In  fact,  all  these  computations  can  be 
done  during  line  sweeping.  If  the  interval  [xj,x2]  is  decomposed  into  sub-intervals, 
the  sweep  line  has  to  be  parallel  to  the  x  axis,  otherwise  it  should  be  parallel  to  the 
y  axis. 

The  overall  line-sweep  process  takes  time  0((n  +  m)  logn),  where  n  is  the  number  of 
C-facets  that  intersect  with  the  interval  (7u,7u+i]  ar>d  m  is  the  number  of  intersections 
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of  the  generalized  polygons.  We  know  that  n  <  n^ng  (see  Subsection  2.2).  On  the 
other  hand,  m  £  0(n2),  but  it  is  usually  much  smaller. 

One  way  to  improve  the  efficiency  of  the  algorithm  could  be  to  restrict  the  line-sweep 
process  to  the  rectangle  [xj,  X2]  x  [t/i,  1/2]-  However,  there  seems  to  be  no  simple  way  of 
establishing  the  initial  status  of  the  sweeping  line  if  it  does  not  start  at  the  bootom- 
most  ordinate  (or  left-most  abscissa).  Nevertheless,  the  process  can  be  stopped  as 
soon  as  the  sweeping  line  leaves  the  rectangle. 

Generalization.  If  A  is  a  non-convex  polygon  that  can  be  decomposed  into  con¬ 
vex  components,  such  that  the  interiors  of  all  these  components  have  a  non-empty 
intersection,  then  the  reference  point  can  be  selected  within  this  intersection  and  the 
above  method  directly  applies  to  each  component  taken  separately. 

If  A  is  non-convex  and  cannot  be  represented  as  the  union  of  overlapping  convex 
components  (for  instance,  it  is  a  (J-shaped  object),  the  above  method  can  still  be 
applied,  but  with  some  changes.  Since  the  reference  point  will  be  outside  some  of  the 
convex  components,  the  shape  of  the  projection  of  the  corresponding  C-facets  will  be 
different,  but  not  difficult  to  establish. 

Avnaim  and  Boissonnat  [Avnaim  and  Boissonnat,  1988]  describe  tin  algorithm  of  time 
complexity  0(n^ng  log  n^ng)  for  computing  the  description  of  the  boundary  of  CB, 
when  both  A  and  B  are  non-convex.  In  the  case  where  A  is  non-convex,  using  this 
algorithm  first  and  projecting  the  C-obstacles  on  the  xy  plane  next,  would  probably 
be  an  efficient  method.  However,  we  have  not  implemented  it. 

3.6  Swept-Area  Method 

Principle.  The  swept-area  method  consists  of  first  computing  two  areas  swept  out 
by  A  when  it  rotates  about  its  reference  point  from  orientation  7„  to  orientation  7U+1: 
the  outer  swept  area  denoted  by  OS *4[7u,7u+i]  and  the  inner  swept  area  denoted  by 
2\S.4[7u, 7u+i]-  They  are  defined  as  follows: 

OSA[yu , 7u+i]  =  U«e[7.,7.+1]-4(°’M)  =  {(r.y)  /  30  £  [7u,7u+i]  :  (*>2/)  €  .4(0, 0,0)}, 

2SA[yu , 7u  +  i]  =  n«€(7.,7.+1]-4(0.M)  =  {(z,y)  /  V0  €  [7u,7u+i]  :  (*.y)  6  .4(0,0, 0)}. 

Then,  the  method  regards  both  OSA[ 7u,7u+i]  and  TSA[iu,  7u+i]  as  moving  objects, 
which  can  only  translate  in  the  plane,  and  it  maps  the  obstacle  B  in  the  configuration 
spaces,  R2,  of  these  objects.  The  mapping  is  obtained  by  “growing”  B  inversely  to 
the  shape  of  these  two  objects,  leading  to  two  regions  OB[-y u,7«+i]  and  X£>[7u,7u+i] 
formally  defined  as  follows  (see  Subsection  2.2): 

OB[  7u,7u+i]  =  S0C?547u,7«+i] 

ZB[ 7u,7u+i]  =  B0I5^[7«,7i.+i]- 
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It  is  shown  in  [Laumond,  1987]  that  the  Minkowski  sum  (resp.  difference)  of  two 
generalized  polygons  is  a  generalized  polygon,  which  can  be  computed  in  time  0(nt  + 
n2),  if  the  two  input  polygons  are  convex,  and  0(nln2),  otherwise.  ( and  n2  denote 
the  number  of  edges  of  the  input  generalized  polygons.) 

The  swept-area  method  returns  two  regions: 

[xi,x2]  x  [yi,y2]  x  05[7U,7U+1] 

and: 

[xi,x2]  x  [yi,y2]  x  XS[7u,7u+i]- 

The  first  is  exactly  OCBxy[Ku}>  as  shown  in  [Lozano- Perez,  1983].  The  second  is 
strictly  included  in  TCBiv[k'1\,  which  leads  the  overall  decomposition  algorithm  to 
generate  a  set  of  FULL  cells  of  less  total  volume  than  with  the  projection  method. 

In  practice,  the  swept-area  method  is  significantly  more  efficient  than  the  projection 
method  when  the  intervals  [7u,7u+i]  are  large.  When  these  intervals  become  small 
enough,  the  projection  method  is  preferred  because  it  exactly  computes  XCBxv[k'1]. 

Computation  of  0<S>l[7u,7u+1].  The  outer  swept  area  C?«S>l[7u,7u+1]  is  a  general¬ 
ized  polygon  (see  Figure  7).  The  straight  edges  of  this  polygon  are  portions  of  edges 
of  >1(0, 0, 7„)  and  >1(0, 0, 7«+i).  Each  circular  edge  is  the  locus  of  a  vertex  of  A,  when 
A  rotates  from  7„  to  ju+1  about  the  reference  point. 

The  contour  of  OSA[ 7u,7«+i]  is  traced  out  in  time  0{n\)  starting  at  the  vertex  the 
most  distant  from  Ox- 

Computation  of  T<Sv4[7tt,7„+i].  The  inner  swept  area  XS  A[iu,~iu+\)  is  a  gener¬ 
alized  polygon  (see  Figure  8).  The  straight  edges  of  this  polygon  are  portions  of 
the  edges  of  >I(0,0,7„)  and  >1(0,0,7U+1).  Each  circular  edge  is  the  locus  of  a  point 
obtained  by  projecting  the  reference  point  on  an  edge  of  >1,  if  that  projection  falls 
between  the  two  extremities  of  the  edge. 

The  contour  of  J<S.4[7U,7U+1]  can  be  traced  out  in  time  0(n2A).  The  starting  point 
is  the  first  intersection  of  a  ray  (any  one)  drawn  from  the  refence  point  with  the 
potential  edges  of  Z«S.4[7u,7u+i].  (Since  the  reference  point  is  in  the  interior  of  A,  it 
is  also  in  the  interior  of  X«S.4[7U,  7„+i].) 
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4  Construction  of  a  Channel 

4.1  First-Cut  Algorithm 

Remember  from  Subsection  2.3  that  a  channel  is  a  sequence  of  adjacent  EMPTY  or 
MIXED  cells  connecting  the  initial  configuration  q,n;t  =  (x,ntt,  yinit,  9,  nit)  to  the  goal 
configuration  q^oaJ  =  {x goal,  y goal,  9 goal)-  A  channel  is  EMPTY,  if  it  only  contains 
EMPTY  cells;  otherwise,  it  is  MIXED.  Below,  we  call  an  EMPTY  channel  an  E- 
channel,  and  a  MIXED  channel  an  M-channel. 

A  simple  first-cut  search  algorithm  for  generating  an  E-channel  is  the  following: 

1.  Generate  a  first  partition  Vo  of  1C.  Construct  the  graph  CCQo  corresponding  to 
this  decomposition.  Set  i  to  0. 

2.  Search  CCQi  for  a  channel.  If  an  E- channel  is  found,  return  success.  If  no  channel 
is  found,  return  failure. 

3.  Let  II  be  the  M-channel  generated  at  Step  2.  Set  Vi+i  to  Vi  and  i  to  i  -I-  1.  For 


21 


Figure  8.  The  contour  of  the  inner  swept  area  of  A  when  it  rotates  about  its  reference 
point  from  orientation  7„  to  orientation  yu+i  is  made  of  portions  of  edges  of  *4(0, 0, 7„)  and 
-4(0,0,  7u-h),  and  circular  segments.  These  circular  segments  are  traced  by  the  projections 
of  Ojj  on  the  edges  of  A  during  the  rotation. 


every  MIXED  cell  k  in  II  partition  k  into  a  set  VK  of  smaller  cells  and  set  Vi  to 
[■p,\«]  U  VK.  Goto  Step  2. 

This  algorithm  searches  successive  ccg’s  until  an  E-channel  is  found.  Each  ccg  CCQi , 
i  ^  0,  is  obtained  from  the  the  previous  ccg,  i.e.  CCQi- 1,  by  only  expanding  some  of 
the  MIXED  nodes,  which  belong  to  the  M-channel  generated  in  CCQi- 1- 

The  search  for  a  channel  in  a  ccg  may  be  guided  by  various  types  of  heuristics.  In 
general,  a  ccg  is  not  searched  for  an  E-channel  before  it  is  searched  for  an  M-channel. 
Indeed,  although  it  is  natural  that  the  heuritics  put  an  extra  cost  on  MIXED  cells  in 
order  to  generate  an  E-channel  quicker,  it  may  also  be  appropriate  to  prefer  shorter 
channels  over  longer  ones  (according  to  some  metrics).  Thus,  although  an  E-channel 
may  exist  in  a  ccg  CCQi ,  it  may  be  preferable  to  generate  a  significantly  shorter  M- 
channel  instead,  and  refine  CCQi  accordingly.  Notice  that  any  E-channel  existing  in 
CCQi  still  exists  in  all  its  successors  CCQi+j,  j  =  1,2, .... 
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4.2  Improved  Algorithm 

The  major  drawback  of  the  simple  first-cut  algorithm  given  above  is  that  the  search 
work  performed  in  CCQi,  if  it  does  not  return  success,  is  not  used  to  help  the  search  of 
CCQi+i-  This  drawback  can  be  remedied  as  follows.  Rather  than  reconstructing  a  full 
ccg,  whenever  MIXED  cells  along  an  M-channel  are  refined,  a  ccg  representing  the 
decomposition  of  every  refined  cell  k  is  generated  separately  and  recursively  searched 
for  a  “subchannel”.  This  subchannel  is  a  sequence  of  adjacent  EMPTY  or  MIXED 
cells  produced  by  the  decomposition  of  k. 

The  new  algorithm  hence  generates  a  hierarchy  of  ccg’s.  The  ccg  at  the  top  of  the 
hierarchy  corresponds  to  the  initial  decomposition  of  C  -  i.e.,  K.  -  and  is  denoted 
by  CCQc ■  Every  other  ccg  corresponds  to  the  decomposition  of  a  certain  MIXED 
cell,  say  k ,  and  is  denoted  by  CCQK.  A  channel  II  is  first  generated  in  CCQc ■  If  II 
is  an  E-channel,  the  planner  exits  with  success;  otherwise,  each  MIXED  cell  k  in  II 
is  decomposed  recursively,  and  a  subchannel  11*,  if  any,  is  generated  in  CCQK.  This 
subchannel  is  substituted  for  k  in  II. 

In  order  to  make  the  algorithm  work  properly,  however,  one  must  be  careful  that  each 
subchannel  II*  connects  appropriately  to  the  rest  of  II  [Kambhampati  and  Davis, 
1986].  This  can  be  worked  out  as  explained  below,  by  generating  a  complete  channel 
connecting  qim<  to  qg0ai  at  every  level  of  refinement. 

Let  II1  be  an  M-channel  extracted  from  the  top-level  ccg,  i.e.  CCQc •  Set  II2  to  the 
empty  sequence  of  cell.  (II2  will  be  incrementally  augmented  into  a  refinement  of  II1.) 
Each  cell  k  in  II1  is  considered  in  the  order  it  appears  in  II1.  If  ic  is  EMPTY,  it  is 
simply  appended  to  the  current  II2.  If  k  is  MIXED,  it  is  first  decomposed  into  a  set 
VK  of  smaller  cells  and  a  ccg  CCQK  is  built  using  this  decomposition;  then,  CCQK  is 
searched  for  a  subchannel  II*  satisfying  the  following  four  conditions: 

-  If  k  is  the  first  cell  in  II1  (hence,  it  contains  q, ■„,*),  then  the  first  cell  in  II*  also 
contains  q,nf(. 

-  If  k  is  the  last  cell  in  II1  (hence,  it  contains  q goai),  then  the  last  cell  in  II*  also 
contains  q^a/- 

-  If  k  is  not  the  first  cell  in  II1,  the  first  cell  of  II*  is  adjacent  to  the  last  cell  of  the 
current  II2. 

-  If  k  is  not  the  last  cell  in  II1,  the  last  cell  of  II*  is  adjacent  to  the  cell  following  k 

in  n1. 

In  the  following,  a  sequence  of  adjacent  EMPTY  or  MIXED  cells  in  the  decomposition 
of  k  is  called  a  subchannel  iff  it  satisfies  these  conditions. 
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(a) 


(b) 


Figure  9.  Figure  a  represents  a  decomposition  of  a  rectangle.  In  order  to  ultimately 
construct  an  El-channel  at  a  deeper  level  of  decomposition,  the  planner  must  consider  the 
sequence  (kj,  *2,  *3!  K2,  k-i),  which  contains  the  cell  *2  twice,  as  an  M-channel.  This  leads 
the  planner  to  decompose  the  two  MIXED  ceils  k2  and  /c3  into  smaller  cells  (for  example, 
as  in  b)  and  generate  the  El-channel  («i,  *21  >  «3i,  *32,  *33,  *22,  *4)- _ 


Assuming  that  the  planner  succeeds  in  generating  II*,  II2  is  modified  by  appending  II* 
to  it.  The  case  when  the  planner  fails  in  generating  II*  will  be  examined  in  Subsection 
4.4. 

When  all  the  cells  in  II1  have  been  considered  successfully,  we  have  obtained  a  channel 
II2,  which  is  a  refinement  of  II1.  If  II2  is  an  E-channel,  the  planning  problem  is  solved; 
the  planner  returns  II2  and  success.  Otherwise,  II2  is  recursively  refined  into  a  third 
channel  II3  by  decomposing  the  MIXED  cells  it  contains.  Etc... 

4.3  Cell  Occurrences  in  a  Channel 

An  El-channel  is  now  generated  by  refining  an  M-channel  without  reproducing  a  global 
ccg.  Therefore,  it  is  important  that  we  allow  an  M-channel  to  contain  several  times 
the  same  MIXED  cell  (i.e.,  loops).  Indeed,  different  occurrences  of  the  same  MIXED 
cell  may  lead  to  different  subchannels.  This  need  is  better  explained  using  an  example. 

Consider  the  two-dimensional  example  of  Figure  9.  In  a,  the  sequence  of  cells 
(*!,  k2,  *3,  *2>  *4),  which  contains  *2  twice,  has  to  be  considered  as  an  M-channel. 
(The  grey  area  represents  C-obstacles,  so  that  k2  and  k3  are  MIXED,  while  and 
*4  are  EMPTY.)  We  assume  that  k2  and  k3  are  decomposed  as  shown  in  Figure  9 
b.  When  the  two  occurrences  of  n2  are  refined  they  produce  different  EMPTY  sub 
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channels.  This  is  illustrated  in  Figure  9  b,  where  k2  is  partitioned  into  three  smaller 
cells,  two  EMPTY  ones  (/c21  and  k22)  and  a  FULL  one  (*%)).  The  first  occurrence  of 
k2  leads  to  a  one-cell  subchannel  made  of  k2i,  while  the  second  occurrence  leads  to 
another  one-cell  subchannel  made  of  k 23 .  On  the  other  hand,  the  decomposition  of 
/c3  produces  four  cells,  three  EMPTY  ones,  /c31,  /c32,  and  /c^,  and  a  FULL  one,  k34. 
From  this  decomposition  the  EMPTY  subchannel  («:31,  ac32, /c33)  is  extracted.  The 
generated  E-channel  is  (/cj ,  /c21,  /c3j ,  /c32,  Kxs,  k22,  k4). 

Since  the  same  cell  may  appear  several  times  in  a  channel  II*,  we  now  refer  to  the 
elements  in  a  channel  as  cell  occurrences  rather  than  just  cells.  We  denote  by  w* 
the  zth  cell  occurrence  in  n*  and  cell( uj)  the  cell  of  which  cj  is  an  occurrence.  A  cell 
occurrence  cj  is  said  MIXED  (resp.  EMPTY)  iff  the  cell  cell(uj)  is  MIXED  (resp. 
EMPTY).  A  subchannel  generated  as  a  refinement  of  a  MIXED  cell  occurrence  u  is 
denoted  by  11^;  it  is  constructed  by  searching  CCQceii(u). 

In  Subsection  4.6,  we  will  examine  the  impact  of  allowing  loops  in  a  channel  on  the 
search  of  a  ccg. 

4.4  Failure  Recovery 

Consider  the  situation  when  the  algorithm  of  Subsection  4.2  fails  to  refine  a  MIXED 
cell  occurrence  cj*  into  a  subchannel.  We  distinguish  three  cases: 

Case  (1): 

r  =  l.  Hence,  cj*  =  cj*  is  the  first  cell  occurrence  in  IT*.  The  search  proceeds 
by  generating  a  new  channel  II*,  i.e.  II*-1  is  re-considered  and  refined  into  a  new 
channel  n*.  If  no  new  channel  II*  can  be  generated  and  k  =  1,  the  planner  returns 
failure.  If  no  new  channel  II*  can  be  generated  and  k  ^  I,  the  planner  recursively 
attempts  to  generate  a  new  channel  II*-1. 

Case  (2): 

i  >  1  and  cj*_j  is  EMPTY.  The  search  proceeds  as  described  in  case  (1).  (Some 
differences  will  appear  in  the  next  subsection.) 

Case  (3): 

1  >  1  and  cj*_,  is  MIXED.  The  search  proceeds  by  generating  another  subchannel 
Il^k  ,  if  any,  in  the  ccg  of  cel/(cj*_,).  If  another  subchannel  exists  in  this  ccg,  the 
planner  substitutes  it  for  the  previously  generated  one  in  Il*+1,  and  tries  again  to 
refine  cj*  into  a  subchannel  by  searching  in  the  ccg  of  ce//(cj*).  Otherwise,  it  iteratively 
treats  w*_,  as  it  just  treated  cj*.  (Either  (1),  (2),  or  (3)  applies  to  cj*_,.) 
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4.5  Recording  Failure  Conditions 

One  can  organize  (sub)channel  generation  and  failure  recovery  in  a  systematic  fashion 
in  order  to  avoid  infinite  looping.  However,  this  may  not  prevent  the  planner  to  make 
the  same  mistakes  several  times.  Higher  efficiency  can  be  obtained  by  remembering 
the  (weakest)  conditions  of  the  failures  in  order  to  avoid  reproducing  them  [Stallman 
and  Sussman,  1977]  [Latombe,  1979].  One  way  to  remember  failure  conditions  is 
to  annotate  cells  and  use  the  annotations  as  follows.  Cases  (1),  (2)  and  (3)  below 
refer  to  the  same  cases  as  in  the  previous  subsection,  denotes  the  number  of  cell 
occurrences  in  n*. 

Case  (1): 

Let  £  =  cell(uji)  and  rp  =  celUto?)  in  the  current  n*.  If  ti*  =  1,  by  convention  rp  =  A, 
where  A  denotes  the  inexisting  cell.  The  cell  £  is  annotated  with  Al  :  [rp\,  which  is 
called  a  type  1  annotation. 

This  annotation  is  later  used  as  follows:  If  a  new  channel  nfc  is  generated,  which 
contains  an  occurrence  cu€  of  £  in  first  position,  then  an  occurrence  of  ip  should  not 
be  considered  a  valid  successor  of  u^.  In  the  particular  case  where  ip  —  A,  this  means 
that  must  not  be  the  last  cell  occurrence  of  the  new  n*. 

Notice  that  the  first  cell  occurrence  in  the  new  H*  is  necessaxily  an  occurrence  of  £, 
since  £  contains  q,mt.  But  there  may  be  other  occurrences  of  £  appearing  in  this  new 
nfc,  to  which  the  above  annotation  does  not  apply. 

Case  (2): 

Let  £  =  cel l (oof)  (i  >  1),  <p  =  cel/(u;^_1)  and  rp  =  cell( w*+1)  in  the  current  nfc.  If 
i  =  nfc,  then  rp  =  A  (with  the  same  meaning  of  A  as  above).  £  is  annotated  with 
A2  :  [p,  rp],  which  is  called  a  type  2  annotation. 

This  annotation  is  later  used  as  follows:  If  a  new  channel  IT*  is  generated,  which 
contains  an  occurrence  u>(  of  £  preceded  by  an  occurrence  of  (p,  then  an  occurrence 
of  rp  should  not  be  considered  a  valid  successor  of  uj^.  In  the  particular  case  where 
rp  =■  A,  this  means  that  must  not  be  the  last  cell  occurrence  of  the  new  nfc. 

Notice  that  if  rp  ^  A  the  construction  is  symmetrical,  i.e.,  if  £  cannot  be  traversed  by  a 
subchannel  connecting  to  rp,  it  can  neither  be  traversed  by  a  subchannel  connecting 
rp  to  v?.  Therefore,  the  annotation  A 2  :  [y>,rp\  is  later  used  commutatively. 

Case  (3): 

In  this  case,  failure  conditions  are  more  involved  than  in  cases  (1)  and  (2),  and 
may  require  sequences  of  cells  of  arbitrary  length  to  be  considered.  The  cell  £  = 
ce//(u;1*_1)  is  annotated  with  an  expression  of  the  form  A3  :  [p,  called 
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type  3  annotation ,  whose  construction  will  be  explained  below. 

This  annotation  is  later  used  as  follows:  If  a  subchannel  IIW{  is  generated  (by  searching 
CCQ()  as  a  refinement  of  an  occurrence  of  £,  while  is  followed  in  the  current 
channel  II*  by  a  sequence  u l5u;2,  of  cell  occurrences,  such  that  for  all  j  €  [l,g] 
cell(ujj )  =  tpj  (if  0,  =  A,  the  condition  cell(utq)  =  A  requires  that  j  be  the  last  cell 
occurrence  in  the  current  II*),  then  the  last  cell  occurrence  in  IIW(  must  not  be  an 
occurrence  of  <p. 

Let  us  now  examine  how  the  annotation  was  built.  Let  0i  =  cell(uj^)  and  02  = 
ce//(u;f+1),  when  the  planner  failed  to  refine  cj*.  If  n*  =  t,  then  02  =  A.  Let  also 
ip  be  the  last  cell  occurrence  in  the  (incomplete)  current  II*+1,  i.e.  ip  is  the  last  cell 
occurrence  of  the  subchannel  IIwt  previously  generated.  For  every  type  3  annotation 
A3  :  [</?',  (0i— 0p)]  attached  to  01?  if  u>*  is  followed  in  the  current  II*  by  the  sequence 
w*+ !,..., uf+p,  with  cell(u>;+j)  =  0'-  for  all  j  G  [l,p]  and  i  +  p  =  kk  +  1  if  0'  =  A,  £ 
is  annotated  with  A3  :  [v?,  (0i020p+i)],  where  0J+1  =  0'-  for  all  j  €  [l,p]-  If  no  type 
3  annotation  has  been  attached  to  f  during  this  iteration  (this  happens  in  particular 
if  no  type  3  annotation  is  currently  attached  with  0 j),  then  f  is  annotated  with 
A3  :  [<p,(0 102)]- 

4.6  Search  of  a  Cell-Connectivity  Graph 

The  planning  algorithm  builds  a  hierarchy  of  ccg’s  and  searches  each  of  these  ccg’s 
separately.  The  ccg  at  the  top  of  the  hierarchy  is  CCQc  and  is  searched  to  generate 
the  channel  II1.  Each  other  ccg  CCQK  represents  the  decomposition  VK  of  a  cell  k 
which  belongs  to  the  parent  ccg  of  CCQK  in  the  ccg  hierarchy. 

Remember  that  C  is  just  a  particular  cell  fC.  We  denote  by  II0  the  channel  that  only 
contains  wj,  with  ce//(u>°)  =  fC.  II1  is  a  refinement  of  II0.  In  the  very  particular  case 
where  there  is  no  C-obstacle  in  the  configuration  space,  then  K  is  an  EMPTY  cell, 
and  II0  is  an  El-channel,  so  that  II1  does  not  have  to  be  generated. 

Let  us  consider  that  we  have  generated  an  M-channel  II*  (k  >  0),  which  we  now 
refine  into  II*+1,  and  that  we  are  currently  considering  the  MIXED  cell  occurrence 
u>*  e  II*.  Let  k  =  ce//(u;*).  If  k  has  already  been  decomposed,  we  just  reuse  the 
ccg  CCGk  that  was  previously  generated;  otherwise,  we  decompose  k  into  a  set  VK  of 
smaller  cells  using  the  method  described  in  Section  3  and  we  build  CCQK.  We  then 
search  CCQK  for  a  subchannel  that  will  be  appended  to  the  tail  of  the  current  II*+1. 

The  search  of  CCQK  consists  of  first  determining  the  possible  initial  and  goal  cells  of 
the  subchannel  to  be  generated: 

-  If  :  =  1,  w*  is  the  first  cell  of  II*.  Then  the  only  possible  initial  cell  in  CCQK  is  the 
cell  of  VK  that  contains  q;nit-  Otherwise,  the  possible  initial  cells  of  CCQK  are  all 
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the  cells  of  VK  which  are  adjacent  to  the  cell  -  call  it  -  of  the  last  cell  occurrence 
u jv  in  the  current  n*+l  and  whose  insertion  in  nfc+1  as  cell  occurrences  succeding 
does  not  violate  any  annotation  of  type  1  or  2  attached  to  <p. 

-  If  i  =  rik,  oj;  is  the  last  cell  occurrence  of  IIfc.  The  only  possible  goal  cell  in  CCQK  is 
the  cell  of  VK  that  contains  q goai.  Otherwise,  the  possible  goal  cells  of  CCQK  are  all 
the  cells  of  VK  which  are  adjacent  to  ceU(u;+1)  and  do  not  violate  any  annotation 
of  type  3  attached  to  k. 

If  there  is  no  possible  initial  or  goal  cell  in  CCQK ,  the  planner  considers  that  situation 
as  a  failure  to  refine  and  applies  the  treatment  described  in  Subsections  4.4  and 
4.5. 

If  at  least  one  initial  and  one  goal  cell  are  established  in  CC@K,  the  planner  searches 
that  graph  for  a  subchannel  11^*  linking  any  of  the  initial  cells  to  any  of  the  goal  cells. 
It  does  that  in  a  way  such  that  no  annotation  of  type  1  or  2  attached  to  the  cells 
in  the  generated  subchannel  is  violated.  (If  *  >  I,  checking  the  annotations  of  type 
2  attached  to  the  first  cell  occurrence  in  this  subchannel  requires  the  cell  of  the  last 
cell  occurrence  in  the  current  IIA:+1  to  be  considered.) 

As  noted  previously,  the  subchannel  IIwi.  to  be  generated  in  CCQK  should  be  allowed 
to  contain  several  occurrences  of  any  MIXED  cell  in  PK.  However,  it  seems  quite 
realistic  in  an  implementation  to  limit  the  number  of  occurrences  of  the  same  cell  in 
a  subchannel7. 

Since  there  may  be  multiple  occurrences  of  the  same  cell  in  a  subchannel,  the  actual 
graph  that  is  searched  to  refine  a  cell  occurrence  w*  is  not  CCQ*,  but  a  graph  derived 
from  it.  This  graph  contains  the  subset  of  subchannels  (with  and  without  loops) 
in  CCQk,  which  do  not  include  two  occurrences  of  the  same  EMPTY  cell  or  more 
occurrences  of  the  same  MIXED  cell  than  a  predefined  number  (see  paragraph  above). 
This  search  graph  is  made  (partly)  explicit  while  it  is  searched.  Since  a  MIXED  cell 
is  adjacent  to  a  finite  number  of  other  cells,  the  search  graph  is  finite.  Hence  its 
search  is  guaranteed  to  terminate,  either  with  success  or  with  failure.  If  it  terminates 
with  success  the  generated  subchannel  is  appended  to  nfc+1.  If  it  terminates  with 
failure,  the  treatment  described  in  Subsections  4.4  and  4.5  is  applied.  Thanks  to  the 
annotations  attached  to  the  cells,  the  search  cannot  iterate  for  ever  through  the  same 
sequence  of  failures. 

7In  our  implementation,  this  number  is  not  bounded.  However,  we  apply  a  best-first  search 
strategy,  which  always  considers  the  (sub)channels  of  minimal  “cost”  first.  The  cost  of  a  (sub)channel 
grows  with  its  length  and  the  number  of  MIXED  cells  it  contains. 
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4.7  Boundary-Connectivity  Graph 

Cell-connectivity  graphs  are  one  way  to  represent  connectivity  among  cells.  Another 
way  is  to  build  a  graph  called  boundary-connectivity  graph,  or  beg.  Given  a 
partition  VK  of  a  cell  k  into  non-overlapping  cells,  the  beg  BCQK  is  defined  as  follows: 

-  Each  node  of  BCQK  corresponds  to  the  intersection  of  the  boundaries  of  two  adjacent 
cells. 

-  Two  nodes  of  BCQK  are  connected  by  a  link  iff  the  two  nodes  are  portions  of  the 
boundary  of  the  same  cell. 

Each  link  in  a  beg  can  be  regarded  as  the  connection  between  two  cells  (each  cell 
includes  the  boundary  portion  associated  with  one  extremity  of  the  link)  through  a 
third  one. 

For  the  same  cell  k,  the  size  of  BCQk  is  larger  than  CCQ^s  size.  However,  beg’s 
presents  some  advantages  over  cog’s  because  they  represent  more  explicitely  the  con¬ 
nectivity  of  the  cells  in  VK.  In  particular,  using  beg’s  removes  the  need  for  type  1 
and  2  annotations.  Indeed,  attaching  a  type  1  or  2  annotation  to  a  cell  in  a  ccg  is 
equivalent  to  removing  a  link  in  a  beg.  However,  in  order  to  use  beg’s  properly  one 
must  be  careful  about  various  details,  hi  particular,  the  initial  and  goal  configura¬ 
tions  should  appear  as  nodes  in  some  of  the  beg’s.  Moreover,  since  type  2  annotations 
may  involve  two  cells  of  two  different  partitions  VKl  and  beg’s  should  also  be 
connected  among  them. 

One  possible  way  to  use  beg’s  is  sketched  below.  Every  M-channel  nfc_1  (k  >  0)  is 
still  a  sequence  of  cell  occurrences.  When  it  is  refined  into  nfc,  an  extended  beg  BCQk 
is  constructed  as  the  beg  of  all  the  cells  which  are  either  EMPTY  cells  in  nfc  or  cells 
.  in  the  decomposition  VK  of  a  MIXED  cell  k  in  n*.  In  addition,  q,nt£  and  q^/  are 
included  in  BCQk  aim  nodes.  The  node  corresponding  to  q,ni£  (resp.  q^/)  is  connected 
by  a  link  to  every  node  representing  a  boundary  portion  of  the  cell  containing  q,ni£ 
(resp.  qg0a;)-  When  n*  is  refined,  parts  of  BCQk  may  already  exist  and  are  simply 
re-used.  The  links  having  q,nit  as  one  extremity  are  called  initial  links.  The  links 
having  q goai  as  one  extremity  are  called  goal  links.  The  links  contained  in  the  beg 
BCQk  of  a  cell  k  are  called  intra-links.  All  the  other  links  are  called  inter-links. 

The  refinement  of  H*-1  into  H*  is  essentially  done  in  the  same  way  as  before.  The 
only  difference  is  that  rather  than  searching  the  ccg  of  the  MIXED  cell  occurrences 
along  II*-1,  the  planner  now  searches  the  portion  of  BCQk  that  corresponds  to  the  beg 
of  the  MIXED  cell  occurrence  being  currently  considered.  The  initial  links,  goal  links, 
and  inter-links  are  used  only  to  determine  the  possible  initial  and  goal  nodes  of  this 
search.  Case  (1)  and  (2)  failures  lead  to  remove  some  links  from  BCQk .  Annotations 
of  type  3  are  still  needed  and  used  as  before. 
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Figure  10.  Example  1. 


5  Implementation  and  Experimentation 

We  have  implemented  the  techniques  described  in  the  above  sections  in  a  path  planner. 
The  planner  is  written  in  Allegro  Common  Lisp  and  runs  on  an  Apple  Macintosh  II 
computer. 

We  ran  the  planner  on  many  examples  including  both  convex  and  non-convex  mov¬ 
ing  objects.  Twelve  of  these  examples  are  illustrated  in  Figures  10  through  21.  In 
each  figure,  we  show  a  path  extracted  from  the  generated  channel.  Statistics  -  CPU 
time,  total  number  of  cells  generated  (iVtotaj),  number  of  EMPTY  cells  generated 
{Nempty),  number  of  EMPTY  cells  used  in  the  output  channel  ( Nused ),  and  decom¬ 
position  efficiency  measure  ( E )  -  characterizing  the  efficiency  of  the  planner  are  given 
in  Table  1. 

Examples  4  and  8  have  been  previously  reported  in  [Brooks  and  Lozano-Perez,  1982] 
(BLP  planner),  the  best  previous  planner  known  to  the  authors  using  the  hierarchical 
approximate  cell  decomposition  approach.  In  Table  2,  we  give  the  comparison  of  the 
statistics  on  these  two  examples  with  our  planner  and  BLP  planner8.  We  should 
point  out  that  for  Example  4  the  path  generated  by  our  planner  is  “simpler”  than  the 

8The  BLP  planner  was  implemented  on  a  LISP  machine,  which  runs  LISP  approximately  5  times 
faster  than  the  Macintosh  II  runs  Allegro  Common  Lisp  (according  to  our  own  benchmark).  In 
addition,  since  we  do  not  count  the  number  of  FULL  cells  in  our  planner’s  statistics  (they  have  no 
effect  on  the  search  graph),  we  subtracted  this  number  from  the  statistics  of  the  BLP  planner. 


30 


Figure  12.  Example  3. 
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Figure  13.  Example  4. 
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Figure  19.  Example  10. 


Figure  20.  Example  11. 


Figure  21.  Example  12. 


path  generated  by  BLP  planner,  in  the  sense  that  it  does  not  include  a  backing-up 
maneuver. 

For  examples  2,  6,  11,  and  12,  we  compared  the  number  of  cells  generated  by  our 
planner  with  the  number  of  cells  that  a  similar  planner  using  the  octree  decomposition 
technique  (see  Subsection  3.2)  would  generate.  Based  on  partial  implementation,  we 
obtained  a  conservative  estimate  (i.e.,  lower  bound)  of  the  number  of  cells  generated 
by  the  octree  decomposition  technique.  The  data  are  reported  in  Table  3.  Example 
11  was  extracted  from  [Avanaim,  Boissonnat  and  Faverjon,  1988].  According  to  this 
paper,  the  planner  described  in  [Faverjon,  1984],  which  uses  the  octree  decomposition 
technique,  generates  more  than  10,000  cells. 

Therefore,  the  above  results  demonstrate  major  improvements  brought  by  our  algo¬ 
rithms  to  the  approximate  cell  decomposition  method. 

In  addition  to  the  previous  experiments,  we  tried  to  characterize  empirically  the 
efficiency  of  the  planner  as  a  function  of  the  “complexity”  of  the  workspace  -  i.e., 
the  number  of  edges  of  the  obstacles  -  and  the  “sparsity”  of  the  workspace  -  i.e., 
the  distance  between  obstacles.  (The  “clutteredness”  of  the  workspace  is  the  inverse 
of  the  sparsity.)  To  that  purpose  we  run  our  planner  on  several  hundreds  problems 
using  randomly  generated  environments  with  pre-specified  complexity  and  sparsity. 
Figure  22  shows  twelve  of  these  examples.  The  vertical  axis  points  down  toward 
higher  complexity  and  the  horizontal  axis  points  right  toward  smaller  sparsity  (i.e., 
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Figure  22.  We  ran  our  planner  on  many  randomly  generated  examples  with  different 
complexity  -  number  of  edges  -  and  sparsity  -  distance  between  obstacles  -  of  the  workspace. 
The  purpose  was  to  gather  statistics  about  the  effiency  of  the  planner  as  a  function  of  these 
parameters. _ 
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Figure  23.  These  diagrams  illustrate  the  variation  of  the  running  time  of  the  planner  as 
a  function  of  the  complexity  (a  and  b)  and  the  sparsity  (c  and  d)  of  the  workspace. _ 
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Table  2:  Comparision  with  the  BLP  Planner 


greater  clutteredness).  (The  two  obstacles  in  the  workspace  are  enclosed  within  two 
rectangular  boxes.  The  sparsity  is  measured  as  the  distance  between  the  vertical 
sides  of  these  two  boxes.)  For  each  complexity  and  sparsity,  we  have  generated  and 
run  tens  of  examples.  The  mean  value  of  the  running  time  obtained  for  some  series 
of  examples  are  depicted  in  Figure  23.  Figure  23  a  corresponds  to  the  first  column 
of  Figure  22,  Figure  23  b  to  the  third  column,  Figure  23  c  to  the  first  row,  and 
Figure  23  d  to  the  third  row.  Figures  23  a  and  b  show  that,  within  the  range  of  our 
experiments,  the  running  time  varies  linearly  with  the  complexity.  Figures  23  c  and 
d  show  that  above  some  clutteredness,  the  running  time  increases  very  quickly.  This 
results  from  the  fact  that  the  planner  has  to  generate  many  small  cells  before  it  has 
any  chance  to  find  an  El-channel. 

6  Conclusion 

We  have  described  new  heuristic  algorithms  for  path  planning  using  the  hierarchical 
approximate  cell  decomposition  approach.  An  important  feature  of  this  approach  is 
that  it  decomposes  the  robot’s  configuration  space  into  cells  of  predefined  shape  - 
'ypically,  rectangloids  -  at  successive  levels  of  approximation.  The  two  main  steps  of 
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Num  cells 

examples 

examples 

example  11 

example  12 

Ours 

Octree 

BH 

218 

>  2000 

312 

>  5000 

389 

>  5000 

Table  3:  Comparison  with  the  Octree  Method 


the  approach  are  cell  decomposition  and  graph  searching. 

With  respect  to  cell  decomposition,  we  have  proposed  an  algorithm  that  decomposes 
a  MIXED  rectangloid  cell  into  a  collection  of  EMPTY,  FULL  and  MIXED  rect- 
angloid  cells.  It  attempts  to  minimize  the  total  volume  of  the  generated  MIXED 
cells,  while  producing  a  reasonably  small  number  of  cells.  Our  algorithm  is  based 
on  the  construction  of  a  bounding  and  a  bounded  rectangloid  approximations  of  the 
C-obstacles.  Experience  shows  that  it  produces  much  better  results  than  with  earlier 
decomposition  techniques.  Additional  improvements  could  be  possible,  for  example 
by  allowing  cells  having  more  involved  shapes.  However,  such  an  improvement  would 
probably  make  other  components  of  the  planner  (e.g.,  the  construction  of  the  search 
graph)  more  delicate  to  implement  and  eventually  less  efficient  to  run. 

With  respect  to  graph  searching,  we  have  proposed  a  set  of  techniques  for  allowing 
the  planner  to  take  advantage  of  the  work  carried  out  at  other  levels  of  detail  for 
conducting  the  search  at  the  current  level  of  decomposition.  Some  of  the  techniques, 
inspired  from  dependency-directed  backtracking,  allows  the  planner  to  record  the 
conditions  of  past  mistakes  so  that  it  does  not  repeat  them. 

We  have  implemented  the  proposed  algorithms  in  a  path  planner,  with  which  we 
have  conducted  a  variety  of  experiments.  These  experiments  show  that  our  planner 
is  significantly  faster  than  previous  planners  based  on  the  same  general  approach. 

Our  current  research  is  aimed  at  developing  a  motion  planner /controller  capable  of 
dealing  with  some  un-expected  obstacles.  Our  approach  described  in  [Choi,  Zhu  and 
Latombe,  1989]  consists  of  controlling  the  robot  in  a  channel  using  a  potential  field 
approach.  We  have  conducted  experiments  with  this  approach  both  with  a  simulated 
robot  and  an  actual  one.  We  currently  work  on  the  interaction  between  the  planner 
and  the  controller,  so  that  if  the  robot  gets  stuck  in  a  channel  (for  example,  the  channel 
may  be  completely  obstructed  by  an  obstacle),  the  controller  may  ask  the  planner  to 
modify  the  channel  accordingly.  This  kind  of  interaction  requires  to  planner  to  be 
fast. 

References 

Aho,  A.V.,  Hopcroft,  J.E.  and  Ullman,  J.D.  (1983)  Data  Structures  and  Algorithms , 
Addison- Wesley,  Reading,  MA. 


40 


Avnaim,  F.  and  Boissonnat,  J.D.  (1988)  Polygon  Placement  Under  Translation  and  Rota¬ 
tion,  Technical  Report  No.  889,  INRIA,  Sophia-Antipolis,  France. 

Ayala,  D.,  Brunet,  P.,  Juan,  R.  and  Navazo,  I.  (1985)  “Object  Representation  by  Means  of 
Nonminimal  Division  Quadtrees  and  Octrees,”  ACM  Transactions  on  Graphics,  4(1). 

Brooks,  R.A.,  and  Lozano-Perez,  T.  (1982)  A  Subdivision  Algorithm  in  Configuration  Space 
for  Findpath  with  Rotation,  AI  Memo  684,  AI  Laboratory,  MIT,  Cambridge,  MA. 

Brooks,  R.A.  and  Lozano-Perez,  T.  (1983)  “A  Subdivision  Algorithm  in  Configuration  Space 
for  Finpath  with  Rotation,”  Eighth  International  Joint  Conference  on  Artificial  Intelligence, 
799-806,  Karlsruhe,  FRG. 

Brost,  R.C.  (1989)  “Computing  Metric  and  Topological  Properties  of  Configuration-Space 
Obstacles,”  IEEE  International  Conference  on  Robotics  and  Automation,  Scottsdale,  Ari¬ 
zona. 

Choi,  W.,  Zhu,  D.J.  and  Latombe,  J.C.  (1989)  “Contingency-Tolerant  Motion  Planning  and 
Control,”  IEEE  Workshop  on  Intelligent  Robots  and  Systems,  IROS’89,  Tsukuba,  Japan. 

Donald,  B.R.  (1984)  Motion  Planning  With  Six  Degrees  of  Freedom,  Report  No.  AI-TR-791, 
Artificial  Intelligence  Laboratory,  MIT,  Cambridge,  MA. 

Faverjon,  B.  (1986)  “Object  Level  Programming  Using  an  Octree  in  the  Configuration 
Space  of  a  Manipulator,”  IEEE  International  Conference  on  Robotics  and  Automation,  San 
Francisco,  CA. 

Gouzenes,  L.  (1984)  “Strategies  for  Solving  Collision-Free  Trajectories  Problems  for  Mobile 
and  Manipulator  Robots,”  International  Journal  of  Robotics  Research,  3(4),  51-65. 

Kambhampati,  S-  and  Davis,  L.S.  (1986)  “Multiresolution  Path  Planning  for  Mobile 
Robots,”  IEEE  Journal  of  Robotics  and  Automation,  RA-2  (3),  135-145. 

Kant,  K.  and  Zucker,  S.W.  (1986)  Planning  Smooth  Collision-Free  Trajectories:  Path,  Ve¬ 
locity  and  Splines  in  Free  Space,  Technical  Report,  Computer  Vision  and  Robotics  Labora¬ 
tory,  McGill  University,  Montreal. 

Khatib,  O.  (1986)  “Real-Time  Obstacle  Avoidance  for  Manipulators  and  Mobile  Robots,” 
International  Journal  of  Robotics  Research,  5(1),  90-98. 

Khosla,  P.  and  Volpe,  R.  (1988)  “Superquadric  Artificial  Potentials  for  Obstacle  Avoidance 
and  Approach,”  IEEE  International  Conference  of  Robotics  and  Automation,  Philadelphia, 
PA,  1178-1784. 

Latombe,  J.C.  (1979)  “Failure  Processing  in  a  System  for  Designing  Complex  Assemblies,” 
Sixth  International  Joint  Conference  on  Artificial  Intelligence  (IJCAIJ,  Tokyo,  Japan. 

Laugier,  C.  and  Germain,  F.  (1985)  “An  Adaptative  Collision-Free  Trajectory  Planner,” 
International  Conference  on  Advanced  Robotics  (ICAR),  Tokyo,  Japan. 

Laumond,  J.P.  (1987).  “Obstacle  Growing  in  a  Non  Polygonal  World,”  Information  Pro¬ 
cessing  Letters,  Information  Processing  Letters,  25,  41-50. 


41 


r 


Lozano-Perez,  T.  (1983)  “Spatial  Planning:  A  Configuration  Space  Approach,”  IEEE 
Transactions  on  Computers,  C-32(2),  108-120. 

Lozano-Perez,  T.  (1987)  “A  Simple  Motion-Planning  Algorithm  for  General  Robot  Manip¬ 
ulators,”  IEEE  Journal  of  Robotics  and  Automation,  RA-3(3),  224-238. 

O’Dunlaing,  C.,  Sharir,  M.  and  Yap,  C.K.  (1983)  “Retraction:  A  New  Approach  to  Motion 
Planning,”  15"1  FOCS,  207-220. 

Preparata,  F.P.  and  Shamos,  M.I.  (1985)  Computational  Geometry:  An  Introduction. 
Springer- Verlag,  New  York. 

Rimon,  E.  and  Koditschek,  D.E.  (1988)  “Exact  Robot  Navigation  using  Cost  Functions:  The 
Case  of  Distinct  Spherical  Boundaries  in  En ,”  IEEE  International  Conference  on  Robotics 
and  Automation,  Philadelphia,  PA,  1791-1796. 

Schwartz,  J.T.  and  Sharir,  M.  (1983)  “On  the  ‘Piano  Movers’  Problem:  II.  General  Tech¬ 
niques  for  Computing  Topological  Properties  of  Real  Algebraic  Manifolds”,  Advances  in 
Applied  Mathematics,  Academic  Press  4,  298-351;  also  in  [Schwartz,  Sharir  and  Hopcroft, 
1987], 

Schwartz,  J.T.,  Sharir,  M.  and  Hopcroft,  J.  (1987)  Planning,  Geometry,  and  Complexity  of 
Robot  Motion.  Ablex,  Norwood,  NJ. 

Spivak,  M.  (1979)  A  Comprehensive  Introduction  to  Differential  Geometry,  Publish  or  Per¬ 
ish,  Wilmington,  DE. 

Stallman,  R.M.  and  Sussman,  G.J.  (1977)  “Forward  Reasoning  and  Dependency-Directed 
Backtracking  in  a  System  for  Computed-Aided  Circuit  Analysis,”  Artificial  Intelligence, 
9(2),  135-196. 


42 


